#include "simstruc.h"
#include "tmwtypes.h"
#include <libserialport.h>
#include <pthread.h>
#include <stdio.h>
#include <unistd.h> 
#include "controller/motor.h"

/* define out port/in port number & width */
int_T nInPortNums = 3;
int_T nInPortNumWidth[3] = {4/*adc*/, 2,/*encoder count/dir*/ 3/*real vel/angle/time*/};
int_T nOutPortNums = 3;
int_T nOutPortNumWidth[3] = {4/*timer*/, 2/*debug-pos*/, 3/*debug-curr*/};

int_T simGetInPortNums(void) {
    return nInPortNums;
}
int_T simGetInPortNumWidth(int port) {
    return nInPortNumWidth[port];
}
int_T simGetOutPortNums(void) {
    return nOutPortNums;
}
int_T simGetOutPortNumWidth(int port) {
    return nOutPortNumWidth[port];
}

#define IN_PORTS(p)             *ssGetInputPortRealSignalPtrs(S,p)
#define Out_PORTS(p)            ssGetOutputPortRealSignal(S,p)

#define in_curr_a_adc 			IN_PORTS(0)[0]
#define in_curr_b_adc         	IN_PORTS(0)[1]
#define in_vbus_curr_adc        IN_PORTS(0)[2]
#define in_vbus_vol_adc         IN_PORTS(0)[3]

#define in_enc_count            IN_PORTS(1)[0]
#define in_enc_dir              IN_PORTS(1)[1]

#define in_mot_vel              IN_PORTS(2)[0]
#define in_mot_angle            IN_PORTS(2)[1]
#define in_sim_clk              IN_PORTS(2)[2]


#define out_pwm_duty1          Out_PORTS(0)[0]
#define out_pwm_duty2          Out_PORTS(0)[1]
#define out_pwm_duty3          Out_PORTS(0)[2]
#define out_pwm_enable         Out_PORTS(0)[3]

#define out_foc_vel            Out_PORTS(1)[0]
#define out_foc_angle          Out_PORTS(1)[1]

#define out_foc_phase_curr     Out_PORTS(2)


/* need extern to bsp */
s16   g_encoder_dir = 1;
s16   g_encoder_counter = 0;
float g_motor_angle;
float g_motor_velocity;;
u16   g_pwm_duty_out[3];
bool  g_pwm_update_irq = false;
bool  g_pwm_chan_enabled = false;
bool  g_adc_started = false;
s16   g_phase_current_adc[2];
s16   g_vbus_current_adc;
s16   g_vbus_voltage_adc;
u32   cpu_cycle_count = 0;
u32   SystemCoreClock = 2000000;

void sim_sfunc_core(SimStruct *S)
{
	static u32 _mc_sched_count = 0;
	g_phase_current_adc[0] = in_curr_a_adc;
	g_phase_current_adc[1] = in_curr_b_adc;
	g_vbus_current_adc = in_vbus_curr_adc;
	g_vbus_voltage_adc = in_vbus_vol_adc;
	cpu_cycle_count = in_sim_clk * SystemCoreClock;
	g_motor_angle = in_mot_angle;
	g_motor_velocity = in_mot_vel;
	g_encoder_counter = (s16)in_enc_count;
	g_encoder_dir = (s16)in_enc_dir;

	if (g_adc_started) {
		motor_adc_irq_handler(motor(0));
	}else if (g_pwm_update_irq) {
		timer_up_irq_handler(motor(0));
	}
	u32 slow_to_pwm_cnt = (CONFIG_FOC_PWM_FREQ / CONFIG_SPEED_CTRL_FREQ);
#if CONFIG_FOC_PWM_UPDATE_REPEAT==0
	slow_to_pwm_cnt *= 2;
#endif
	if (++_mc_sched_count == slow_to_pwm_cnt) {
		_mc_sched_count = 0;
		mc_sched_irq_handler(motor(0));
#if CONFIG_USE_MAG_ENCODER
		float t = g_motor_angle / M_2PI;
		enc_pwm_irq_handler(t, 1.0f);
#endif

	}

}

void sim_sfunc_output(SimStruct *S)
{
	foc_t *foc = &controller(motor(0))->foc;
	out_pwm_duty1 = g_pwm_duty_out[0];
	out_pwm_duty2 = g_pwm_duty_out[1];
	out_pwm_duty3 = g_pwm_duty_out[2];

	out_pwm_enable = g_pwm_chan_enabled;
	
	out_foc_vel = controller(motor(0))->encoder_velocity;
	out_foc_angle = controller(motor(0))->encoder_angle;

	out_foc_phase_curr[0] = controller(motor(0))->phase_curr[0];
	out_foc_phase_curr[1] = controller(motor(0))->phase_curr[1];
	out_foc_phase_curr[2] = controller(motor(0))->phase_curr[2];
}
